﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using LaserRadar2;


namespace LaserRadar
{
    internal partial class Form_FindSphere : Form
    {
        //new public CDlgInspect MdiParent { set; get; }
        public double AmoveTo = 0;
        public double BmoveTo = 0;
        public double currentA = 0;
        public double currentB = 0;
        public double currentDis = 0;
        public double currentLight = 0;
        public double step = 0;
        public List<OriginData> rectPoints = new List<OriginData>();
        static Thread findThread;

        private  DeviceRadar m_Radar;
        //private CReceiver m_Receiver;
        //private IM.CIMHost m_host;
        public Form_FindSphere()
        {
            InitializeComponent();
        }
        public Form_FindSphere(DeviceRadar radar)
        {
            InitializeComponent();
            this.m_Radar = radar;
        }
        //public Form_FindSphere(CReceiver receiver_)
        //{
        //    InitializeComponent();
        //    this.m_Receiver = receiver_;
        //    this.m_Radar = receiver_.m_stub.m_Radar;
        //    this.m_host = receiver_.Host;
        //}

        public void showFwData(List<OriginData> fsPoints, List<OriginData> maxs, List<OriginData> mins)
        {
            //List<OriginData> maxs = new List<OriginData>();
            //List<OriginData> mins = new List<OriginData>();
            //findPeaks(fsPoints, maxs, mins);
            if (fsPoints.Count > 0)
            {
                double aCenter = 0, lightmax = 0;
                chartControl1.Invoke(new EventHandler(delegate
                {
                    chartControl1.Series[0].Points.Clear();
                    chartControl1.Series[1].Points.Clear();
                    chartControl1.Series[2].Points.Clear();
                }));
                foreach (OriginData fs in fsPoints)
                {
                    chartControl1.Invoke(new EventHandler(delegate
                    {
                        chartControl1.Series[0].Points.Add(new DevExpress.XtraCharts.SeriesPoint(fs.a, fs.light));
                    }));
                    if (fs.light > lightmax)
                    {
                        lightmax = fs.light;
                        aCenter = fs.a;
                    }
                }
                foreach (OriginData fs in maxs)
                {
                    chartControl1.Invoke(new EventHandler(delegate
                    {
                        chartControl1.Series[1].Points.Add(new DevExpress.XtraCharts.SeriesPoint(fs.a, fs.light));
                    }));
                }
                foreach (OriginData fs in mins)
                {
                    chartControl1.Invoke(new EventHandler(delegate
                    {
                        chartControl1.Series[2].Points.Add(new DevExpress.XtraCharts.SeriesPoint(fs.a, fs.light));
                    }));
                }
                textBox_fwCenter.Invoke(new EventHandler(delegate
                {
                    textBox_fwCenter.Text = aCenter.ToString("0.0000");
                }));
                textBox_fwLight.Invoke(new EventHandler(delegate
                {
                    textBox_fwLight.Text = lightmax.ToString("0.000");
                }));
            }
        }
        public void findPeaks(List<OriginData> data, List<OriginData> maxs, List<OriginData> mins)
        {

            List<int> sign = new List<int>();

            for (int i = 1; i < data.Count; i++)
            {
                double diff = data[i].light - data[i - 1].light;
                if (diff > 0)
                {
                    sign.Add(1);
                }
                else if (diff < 0)
                {
                    sign.Add(-1);
                }
                else
                {
                    sign.Add(0);
                }
            }
            //再对sign相邻位做差  
            //保存极大值和极小值的位置  
            List<int> indMax = new List<int>();
            List<int> indMin = new List<int>();

            for (int j = 1; j < sign.Count; j++)
            {
                int diff = sign[j] - sign[j - 1];
                if (diff < 0)
                {
                    indMax.Add(j);
                    maxs.Add(data[j]);
                }
                else if (diff > 0)
                {
                    indMin.Add(j);
                    mins.Add(data[j]);
                }
            }

        }
        public void showFyData(List<OriginData> fsPoints, List<OriginData> maxs, List<OriginData> mins)
        {
            //List<OriginData> maxs = new List<OriginData>();
            //List<OriginData> mins = new List<OriginData>();
            //findPeaks(fsPoints, maxs, mins);
            if (fsPoints.Count > 0)
            {
                double bCenter = 0, lightmax = 0;
                chartControl2.Invoke(new EventHandler(delegate
                {
                    chartControl2.Series[0].Points.Clear();
                    chartControl2.Series[1].Points.Clear();
                    chartControl2.Series[2].Points.Clear();
                }));
                foreach (OriginData fs in fsPoints)
                {
                    chartControl2.Invoke(new EventHandler(delegate
                    {
                        chartControl2.Series[0].Points.Add(new DevExpress.XtraCharts.SeriesPoint(fs.b, fs.light));
                    }));
                    if (fs.light > lightmax)
                    {
                        lightmax = fs.light;
                        bCenter = fs.b;
                    }
                }
                foreach (OriginData fs in maxs)
                {
                    chartControl2.Invoke(new EventHandler(delegate
                    {
                        chartControl2.Series[1].Points.Add(new DevExpress.XtraCharts.SeriesPoint(fs.b, fs.light));
                    }));
                }
                foreach (OriginData fs in mins)
                {
                    chartControl2.Invoke(new EventHandler(delegate
                    {
                        chartControl2.Series[2].Points.Add(new DevExpress.XtraCharts.SeriesPoint(fs.b, fs.light));
                    }));
                }
                textBox_fyCenter.Invoke(new EventHandler(delegate
                {
                    textBox_fyCenter.Text = bCenter.ToString("0.0000");
                }));
                textBox_fyLight.Invoke(new EventHandler(delegate
                {
                    textBox_fyLight.Text = lightmax.ToString("0.000");
                }));
            }
        }

        private void button_moveToCenter_Click(object sender, EventArgs e)
        {
            float angleA = (float)Convert.ToDouble(textBox_fwCenter.Text);
            float angleB = (float)Convert.ToDouble(textBox_fyCenter.Text);
            int moveSpeed = 1000000;
            if (this.m_Radar != null)
                this.m_Radar.MoveAB(angleA, angleB, moveSpeed);
        }

       
        private void ReceiverdFindCenterCallBack(double a,double b, List<OriginData> data,List<OriginData> maxs, List<OriginData> mins,int axis)
        {
            this.Invoke(new EventHandler(delegate
            {
                //if (filterOpen)
                //{
                //    List<OriginData> dataAfterFiltered = filter(data);
                //    if (axis == 0)
                //    {
                //        showFwData(dataAfterFiltered);
                //    }
                //    else if (axis == 1)
                //    {
                //        showFyData(dataAfterFiltered);
                //    }
                //    textBox_pathPoints.AppendText(a.ToString("f4") + "," + b.ToString("f4") + "\n\t");
                //}
                //else
                //{
                //    if (axis == 0)
                //    {
                //        showFwData(data);
                //    }
                //    else if (axis == 1)
                //    {
                //        showFyData(data);
                //    }
                //    textBox_pathPoints.AppendText(a.ToString("f4") + "," + b.ToString("f4") + "\n\t");
                //}
                if (axis == 0)
                {
                    showFwData(data,maxs,mins);
                }
                else if (axis == 1)
                {
                    showFyData(data, maxs, mins);
                }
                textBox_pathPoints.AppendText(a.ToString("f4") + "," + b.ToString("f4") + "\r\n");
            }));
        }

        private void button_findFWcenter_Click(object sender, EventArgs e)
        {
            chartControl1.Series[0].Points.Clear();
            chartControl1.Series[1].Points.Clear();
            chartControl1.Series[2].Points.Clear();
            OriginData oneD = this.m_Radar.GetOneData();
            pathPoint startPoint = new pathPoint(oneD.a, oneD.b);
            double step = Convert.ToDouble(textBox_fw_step.Text);         
            double speed = Convert.ToDouble(textBox_huiSpeed.Text);
            if (this.m_Radar != null)
            {
                var s = Task.Run(() => this.m_Radar.FindFWCenter(startPoint, step, speed,0.7,3));
            }
        }

        private void button_findFYcenter_Click(object sender, EventArgs e)
        {
            chartControl2.Series[0].Points.Clear();
            chartControl2.Series[1].Points.Clear();
            chartControl2.Series[2].Points.Clear();
            OriginData oneD = this.m_Radar.GetOneData();
            pathPoint startPoint = new pathPoint(oneD.a, oneD.b);
            double step = Convert.ToDouble(textBox_fy_step.Text);
            
            double speed = Convert.ToDouble(textBox_huiSpeed.Text);
            if (this.m_Radar != null)
            {
                Task findTask = new Task(() =>
                {
                    this.m_Radar.FindFYCenter(startPoint, step, speed,0.7,3);
                });
                findTask.Start();
            }                
        }
        int index = 0;
        double[] aArray = new double[1000];
        double[] bArray = new double[1000];
        double[] disArray = new double[1000];
        double[] lightArray = new double[1000];
        public void getData(double a, double b, double dis, double light, double focusDis, double b2, double dis2, double light2)
        {
            aArray[index] = a;
            bArray[index] = b;
            disArray[index] = dis;
            lightArray[index] = light;
            index++;
            if (index == 1000)
            {
                index = 0;

                double fyTemp = Class_Filter.sigma3Filter(bArray);
                double fwTemp = Class_Filter.sigma3Filter(aArray);
                double disTemp = Class_Filter.sigma3Filter(disArray);
                double lightTemp = Class_Filter.sigma3Filter(lightArray);
                textBox_fw.Invoke(new EventHandler(delegate
                {
                    textBox_fw.Text = fwTemp.ToString("0.0000");
                }));
                textBox_fy.Invoke(new EventHandler(delegate
                {
                    textBox_fy.Text = fyTemp.ToString("0.0000");
                }));
                

                double radius = Convert.ToDouble(textBox_radius.Text) / 2;
                disTemp += radius;
                textBox_dis.Invoke(new EventHandler(delegate
                {
                    textBox_dis.Text = disTemp.ToString("0.0000");
                }));
                double x = 0, y = 0, z = 0;
                if (this.m_Radar.compensate3D == false)
                    DeviceRadar.ComputeXYZ_noComse(fwTemp, fyTemp, disTemp, ref x, ref y, ref z);
                else
                    DeviceRadar.ComputeXYZ(fwTemp, fyTemp, disTemp, ref x, ref y, ref z);
                textBox2.Invoke(new EventHandler(delegate
                {
                    textBox2.Text = x.ToString("0.000");
                }));
                textBox3.Invoke(new EventHandler(delegate
                {
                    textBox3.Text = y.ToString("0.000");
                }));
                textBox4.Invoke(new EventHandler(delegate
                {
                    textBox4.Text = z.ToString("0.000");
                }));

                StreamWriter writer = new StreamWriter("centers.txt", true);
                writer.WriteLine(fwTemp + "," + fyTemp + "," + disTemp + ","
                    + x.ToString("0.000") + "," + y.ToString("0.000") + "," + z.ToString("0.000"));
                writer.Close();
                this.m_Radar.SendData -= new SendDataCallback(getData);
            }
        }
        private void button_readCenter_Click(object sender, EventArgs e)
        {
            this.m_Radar.SendData += new SendDataCallback(getData);


            //List<OriginData> centerList = new List<OriginData>();
            //double fy  =0;
            //double fw = 0;
            //double dis = 0;
            //double light = 0;

            //for (int i = 0; i < 1000; i++)
            //{
            //    OriginData aData = this.MdiParent.m_Rander.getOneData();
            //    fy += aData.b;
            //    fw += aData.a;
            //    dis += aData.dis;
            //    light += aData.light;
            //}
            
            //fy = fy / 1000;
            //fw = fw / 1000;
            //dis = dis / 1000; ;
            //light = light /1000;
            //textBox_fw.Text = fw.ToString("0.0000");
            //textBox_fy.Text = fy.ToString("0.0000");
            //textBox_light.Text = light.ToString();
            //textBox_dis.Text = dis.ToString("0.0000");
            //double radius = Convert.ToDouble(textBox_radius.Text) / 2;
            //dis += radius;
            //double x = 0, y = 0, z = 0;
            //DeviceRander.computeXYZ_noComse(fw, fy, dis, ref x, ref y, ref z);
            //textBox2.Text = x.ToString("0.000");
            //textBox3.Text = y.ToString("0.000");
            //textBox4.Text = z.ToString("0.000");

            //StreamWriter writer = new StreamWriter("centers.txt",true);
            //writer.WriteLine(fw+","+fy+","+ dis+"," 
            //    +x.ToString("0.000") + ","+ y.ToString("0.000") + ","+ z.ToString("0.000"));
            //writer.Close();

        }
      
        private void AutoFindProcess()
        {
            OriginData oneD = this.m_Radar.GetOneData();
            currentA = oneD.a;
            currentB = oneD.b;
            pathPoint startPoint = new pathPoint(currentA, currentB);
         
            double speed = Convert.ToDouble(textBox_huiSpeed.Text);
            double lightThreshold = Convert.ToDouble(textBox_treadhood.Text);
            int peaksLimit = Convert.ToInt32(this.textBox_peakCount.Text);         
            this.m_Radar.AutoFindByCross(startPoint, speed,lightThreshold,peaksLimit);
        }
        private void AutoFindStop()
        {

            this.m_Radar.AutoFindByCrossStop();
        }


        private void button_stopFindSphere_Click(object sender, EventArgs e)
        {
            findThread.Suspend();
        }


    

        private void Form_FindSphere_Load(object sender, EventArgs e)
        {
            if(this.m_Radar!=null)
                this.m_Radar.SendFindSphereData += new FindSphereCallback(ReceiverdFindCenterCallBack);
        }

        private void Form_FindSphere_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (this.m_Radar != null)
                this.m_Radar.SendFindSphereData -= new FindSphereCallback(ReceiverdFindCenterCallBack);
        }
       
        private void Button_saveCenter_Click(object sender, EventArgs e)
        {
            try
            {
                double x = Convert.ToDouble(this.textBox2.Text);
                double y = Convert.ToDouble(this.textBox3.Text);
                double z = Convert.ToDouble(this.textBox4.Text);
                string pointPara = String.Format("{0},{1},{2}", x, y, z);
                //m_host.ExecuteCommand(IM.Utils.CommandBuild("FEATURE PRIMITIVE POINT CREATE", pointPara), true, false, true);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }
        }

        private void Button_resetNum_Click(object sender, EventArgs e)
        {
           
        }
        bool filterOpen = false;
        private void button_filterTest_Click(object sender, EventArgs e)
        {
            OpenFileDialog ofd = new OpenFileDialog();
            if (ofd.ShowDialog() == DialogResult.OK)
            {

                FileStream fs = null;
                StreamReader sr = null;
                double[] dataArray = new double[4];
                int cnt = 0;
                string sLineBuffer;
                string[] sABR;
                fs = new FileStream(ofd.FileName, FileMode.Open, FileAccess.Read, FileShare.ReadWrite);
                sr = new StreamReader(fs);
                List<OriginData> data = new List<OriginData>();
                while (!sr.EndOfStream)
                {
                    sLineBuffer = sr.ReadLine();
                    cnt++;
                    sABR = sLineBuffer.Split(chDelimiter, StringSplitOptions.RemoveEmptyEntries);
                    if (sABR == null)
                    {
                        MessageBox.Show("数据出错");
                    }
                    try
                    {
                        dataArray[0] = Convert.ToDouble(sABR[0]);
                        dataArray[1] = 0;
                        dataArray[2] = Convert.ToDouble(sABR[2]);
                        dataArray[3] = Convert.ToDouble(sABR[1]);
                        data.Add(new OriginData(dataArray[0], 0, dataArray[2], dataArray[3]));
                    }
                    catch
                    {
                        continue;
                    }
                }
                if (filterOpen)
                {
                    List<OriginData> dataAfterFiltered = filter(data);
                    //showFwData(dataAfterFiltered);
                }
                else
                {
                    //showFwData(data);
                }

            }

                //double[] d = new double[40];
                //for (int i = 0; i < 40; i++)
                //{
                //    d[i] = 0.01 * (i - 20) * (i - 20);
                //}
                //d[3] = 10;
                //d[15] = -3;
                //d[25] = 20;
                //d[30] = -20;
                //List<OriginData> testData = new List<OriginData>();
                //StreamWriter sw = new StreamWriter("test40.txt");
                //for (int i = 0; i < 40; i++)
                //{
                //    testData.Add(new OriginData(i, 0, 1, d[i]));
                //    sw.WriteLine(d[i]);
                //}
                //sw.Close();
                //if (filterOpen)
                //{
                //    List<OriginData> dataAfterFiltered = filter(testData);
                //    showFwData(dataAfterFiltered);
                //}
                //else
                //{
                //    showFwData(testData);
                //}
        }
        static char[] chDelimiter = new char[] { ' ', '\t', ';', ',', ':', '+' };
        public List<OriginData> filter(List<OriginData> data)
        {
            List<OriginData> dataFilted = new List<OriginData>();
            int n = data.Count;
            double[] orign = new double[n];
            double[] after = new double[n];
            for (int i = 0; i < n; i++)
            {
                orign[i] = data[i].light;
            }
            Class_Filter.medianFilter(orign, after, n, 9, null);

            for (int i = 0; i < n; i++)
            {
                dataFilted.Add(new OriginData(data[i].a, data[i].b, data[i].dis, after[i]));
            }
            return dataFilted;
        }


        private void checkEdit_filter_CheckedChanged(object sender, EventArgs e)
        {
            if (checkEdit_filter.Checked == true)
            {
                filterOpen = true;
            }
            else
            {
                filterOpen = false;
            }
        }

        private void button_crossFind_Click(object sender, EventArgs e)
        {
            Thread autoFindThread = new Thread(AutoFindProcess);
            autoFindThread.Start();
        }

        private void button_autoFindByCrossStop_Click(object sender, EventArgs e)
        {
            this.m_Radar.AutoFindByCrossStop();
        }

        private void checkEdit1_CheckedChanged(object sender, EventArgs e)
        {
            if (checkEdit1.Checked)
                this.m_Radar.compensate3D = true;
            else
                this.m_Radar.compensate3D = false;
        }

        private void button1_Click_1(object sender, EventArgs e)
        {
            try
            {
                double a = Convert.ToDouble(this.textBox_fw.Text);
                double b = Convert.ToDouble(this.textBox_fy.Text);
                double dis = Convert.ToDouble(this.textBox_dis.Text);
                double x = 0, y = 0, z = 0;
                if (this.m_Radar.compensate3D == false)
                    DeviceRadar.ComputeXYZ_noComse(a, b, dis, ref x, ref y, ref z);
                else
                    DeviceRadar.ComputeXYZ(a, b, dis, ref x, ref y, ref z);
                textBox2.Text = x.ToString("f3");
                textBox3.Text = y.ToString("f3");
                textBox4.Text = z.ToString("f3");


            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }

        }

        private void chartControl1_Click(object sender, EventArgs e)
        {

        }
    }
}
